clear all;
close all;


m=1.12;g=9.81;
Ix=0.0119;Iy=0.0119;Iz=0.0223;
b=0.00000773213;d=0.000000127513;Jr=0.00085;l=0.23;
kp1=1.2;kp2=1.2;kp3=1.2;kp4=5.62;
kint1=3.1;kint2=3.1;kint3=3.1;kint4=7.2;
kd1=0.08;kd2=0.08;kd3=0.03;kd4=1.0;
beta1=1.0;beta2=1.0;beta3=3;beta4=2.0;
lamb1=1.2;lamb2=1.2;lamb3=4;lamb4=1.01;
gama1=0.72;gama2=0.72;gama3=0.21;gama4=22;
OHM=1000;

Ixx=0.0119;Iyy=0.0119;Izz=0.0223;
a1=(Iyy-Izz)/Ixx;a2=Jr/Ixx;
a3=(Izz-Ixx)/Iyy;a4=-Jr/Iyy;
a5=(Ixx-Iyy)/Izz;b1=l/Ixx;
b2=l/Iyy;b3=l/Izz;

Qk=[10;0;10;0;5;0;0;0;0;0;0;0];% ode45不能直接用矩阵，得变成向量进行操作；依次是phi,th,fi,h
% Zk=[0;0;0;0;0;0;0;0;0.72;0.72;0.21;22];
Zk=[0;0;0;0;0;0;0;0];
% v=[0;0;0;0];
% Zk=[0;0;0];

% rho1=0.2;
% rho2=0.5;
% rho3=0.4;
% rho4=0.4;
% rho5=0.15;
% rho6=0.4;
% Ta=10;
% Tb=10;
% Tc=10;
% Td=10;
% Te=10;
% Tf=10;
% 
% c1=2^(-1+rho1/2);
% c2=3^(rho1/2)*2^(-1-rho1/2);
% c3=2^(-1+rho2/2);
% c4=3^(rho2/2)*2^(-1-rho2/2);

% c1=1;
% c2=1;
% c3=1;
% c4=1;



% alpha=1.1;
% beta=1.1;
% 
% kai1=20;
% kai2=0.1;
% lem=0.3;
% eta=1;
% delta_est_part1=0;
% delta_est_part2=0;
% delta_est_part3=0;
% 
% uwin=zeros(4,5);



T=0.001;
for k=1:1:12000
  
    time(k)=k*T;

%     xd(k)=3*sin(k*T);
%     dxd(k)=3*cos(k*T);
%     ddxd(k)=-3*sin(k*T);
%     yd(k)=3*sin(2*k*T);
%     dyd(k)=6*cos(2*k*T);
%     ddyd(k)=-12*sin(2*k*T);
%     phid(k)=3*cos(k*T);
%     dphid(k)=-3*sin(k*T);
%     ddphid(k)=-3*cos(k*T);

%     xd(k)=2*sin(0.1*k*T);
%     dxd(k)=0.2*cos(0.1*k*T);
%     ddxd(k)=-0.02*sin(0.1*k*T);
%     yd(k)=2*sin(0.2*k*T);
%     dyd(k)=0.4*cos(0.2*k*T);
%     ddyd(k)=-0.08*sin(0.2*k*T);
%     phid(k)=0;
%     dphid(k)=0;
%     ddphid(k)=0;


    phid(k)=0;
    thd(k)=0;
    fid(k)=0;
    hd(k)=5;
    
    Qd(:,k)=[phid(k);thd(k);fid(k);hd(k)];             
     
%     if(k*T>=5)
%         dt(1)=2*cos(2*pi/3*k*T);
%         dt(2)=7+2*cos(2*pi/3*k*T);
%         dt(3)=5+2*cos(pi/2*k*T);
%         dt(4)=18+4*cos(pi/6*k*T);
%     else
        dt(1)=0;dt(2)=0;dt(3)=0;dt(4)=0;
%     end

    phi(k)=Qk(1);dphi(k)=Qk(2);th(k)=Qk(3);dth(k)=Qk(4);
    fi(k)=Qk(5);dfi(k)=Qk(6);h(k)=Qk(7);dh(k)=Qk(8);
    x(k)=Qk(9);dx(k)=Qk(10);y(k)=Qk(11);dy(k)=Qk(12);

    x1=Qk(1);x2=Qk(2);x3=Qk(3);x4=Qk(4);
    x5=Qk(5);x6=Qk(6);x7=Qk(7);x8=Qk(8);
    x9=Qk(9);x10=Qk(10);x11=Qk(11);x12=Qk(12);
    
    s1=Zk(1);ds1=Zk(2);s2=Zk(3);ds2=Zk(4);
    s3=Zk(5);ds3=Zk(6);s4=Zk(7);ds4=Zk(8);
%     gama1=Zk(9);gama2=Zk(10);gama3=Zk(11);gama4=Zk(12);

    Q(:,k)=[phi(k);th(k);fi(k);h(k)];
    dQ(:,k)=[dphi(k);dth(k);dfi(k);dh(k)];
    
    
    e(:,k) = Qd(:,k)-Q(:,k);
    e1=e(1,k);e2=e(2,k);e3=e(3,k);e4=e(4,k);
    de(:,k) = -dQ(:,k);
    de1=de(1,k);de2=de(2,k);de3=de(3,k); de4=de(4,k);
    

    vt1(k)=m/(kd4*(cos(x1)*cos(x3)))*(-beta4*ds4+kp4*de4+kint4*e4+kd4*g+lamb4*s4+gama4*sign(ds4));
    vt2(k)=1/(kd1*b1)*(-beta1*ds1+kp1*de1+kint1*e1+kd1*(-x4*x6*a1-x4*a2*OHM)+lamb1*s1+gama1*sign(ds1));
    vt3(k)=1/(kd2*b2)*(-beta2*ds2+kp2*de2+kint2*e2+kd2*(-x2*x6*a3-x2*a4*OHM)+lamb2*s2+gama2*sign(ds2));
    vt4(k)=1/(kd3*b3)*(-beta3*ds3+kp3*de3+kint3*e3+kd3*(-x2*x4*a5)+lamb3*s3+gama3*sign(ds3));        
    
    U=[vt1(k);vt2(k);vt3(k);vt4(k)];

    tSpan = [0 T];
	[tt,Qq] = ode45(@(t,x) plant(t,x,U),tSpan,Qk);
    Qk = Qq(length(Qq),:);
   	[tt1,Zq] = ode45(@(t,x) plants(t,x,dt),tSpan,Zk);
    Zk = Zq(length(Zq),:);
                              
                                   
     
end

figure(1);
plot(time,phi,'k',time,phid,'r--');
xlabel('time(s)');ylabel('phi');
legend('φq','φd');
grid on;

figure(2);
plot(time,th,'k',time,thd,'r--');
xlabel('time(s)');ylabel('th');
legend('θq','θd');
grid on;
 
figure(3);
plot(time,fi,'k',time,fid,'r--');
xlabel('time(s)');ylabel('fi');
legend('ψq','ψd');
grid on;
 
figure(4);
plot(time,h,'k',time,hd,'r--');
xlabel('time(s)');ylabel('h');
legend('hq','hd');
grid on;

figure(5);
plot(time,vt1,'k');
xlabel('time(s)');ylabel('U1');
grid on;

figure(6);
plot(time,vt2,'k');
xlabel('time(s)');ylabel('U2');
grid on;

figure(7);
plot(time,vt3,'k');
xlabel('time(s)');ylabel('U3');
grid on;

figure(8);
plot(time,vt4,'k');
xlabel('time(s)');ylabel('U4');
grid on;
